/*----------------------------------------------------------------------------*/
/* Team 3647
/* Shooter.java
/*----------------------------------------------------------------------------*/
package Robot;

import edu.wpi.first.wpilibj.CounterBase;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.Timer;

public class Shooter {

    private static final Servo cameraPan            = new Servo(10);
    private static final LinearVictor shooterVictor = new LinearVictor(6);
    private static final Encoder shooterEncoder     = new Encoder(4,5, true, CounterBase.EncodingType.k1X);
    
    private static double degrees = .77;
    public static void PanCamera() {
        if (DriveTrain.OpStick.getRawButton(11)) {
            degrees += .01;
            if (degrees > 1) {
                degrees = 1;
            }
        } else if (DriveTrain.OpStick.getRawButton(10)) {
            degrees -= .01;
            if (degrees < 0) {
                degrees = 0;
            }
        }
        cameraPan.set(degrees);
    }
    
    private static double prevCounts = 0, prevTime = 0, rate = 0;
    private static final double a = .5;
    private static final int tol = 10; // Tolerance
    private static int GetRate() {
        double currCounts = shooterEncoder.get();
        double currTime = Timer.getFPGATimestamp();
        double newRate = Math.abs(currCounts - prevCounts) / Math.abs(currTime - prevTime);
        prevCounts = currCounts;
        prevTime = currTime;
        newRate /= 6.0; //covert to RPM
        rate = rate * a + (1 - a) * newRate;
        rate = ((int) (rate / tol)) * tol;
        return (int)rate;
    }
    
    private static double errorThreshold = 30, fullPower = 1.0, reducedPower = 0.76;              
    ///private static double errorThreshold = 30, fullPower = .9, reducedPower = 0.55;              
    public static void BangBang(double setpoint) {
        double error = setpoint - GetRate();   
        if (error > errorThreshold) {     //If the error is larger than the threshold
            shooterVictor.set(fullPower);
        } else if (setpoint != 0.0) {     //If the setpoint is nonzero, and the error is within the threshold
            shooterVictor.set(reducedPower);  
        } else {
            shooterVictor.set(0);              
        }
    }
     
    private static double keyRPM = 2650; 
    public static void RunShooter() {
        if (DriveTrain.OpStick.getRawButton(3)) {
            BangBang(keyRPM);
            Tower.StopOut();
        } else {
            BangBang(0);
            //Tower.StopOut();
        }
    }
    
    public static void StartEncoder() {
        shooterEncoder.start();
        shooterEncoder.setDistancePerPulse(1.0/360.0);
    }
   
    public static void ResetShooter() {
        shooterEncoder.reset();
        prevCounts = 0;
        prevTime = 0;
        rate = 0;
    }

}
